//========================================================================
//  This software is free: you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License Version 3,
//  as published by the Free Software Foundation.
//
//  This software is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public License
//  Version 3 in the file COPYING that came with this distribution.
//  If not, see <http://www.gnu.org/licenses/>.
//========================================================================
/*!
\file    skeleton_pointing.cpp
\brief   C++ Implementation: SkeletonPointing, main
\author  Joydeep Biswas, (C) 2011
*/
//========================================================================

#include "skeleton_pointing.h"


SkeletonPointing pointingThread;


void SkeletonPointing::statusCallback(void* data)
{
  static const bool debug = false;
  if(debug) printf("GUI updated!\n");
  //display->updateDisplay(vector2d(0.0,0.0),msg.robotAngle, 10.0, lines, points, circles, lineColors, pointColors, circleColors);
}

void SkeletonPointing::testDisplay()
{
  while(runApp){
    //TODO: Write a simple test to demo the drawing capabilities
    Sleep(0.016);
  }
}

void SkeletonPointing::skeletonUpdate(NUI_SKELETON_FRAME* data)
{
  //printf("Skeleton Data received!\n");
  display->updateData(*data);
  pointingEngineMutex.lock();
  pointingEngine.observe(data);
  pointingEngineMutex.unlock();
}

void SkeletonPointing::startCalibration(int pointNum)
{
  pointingEngineMutex.lock();
  pointingEngine.startCalibration(pointNum);
  pointingEngineMutex.unlock();
}

void SkeletonPointing::stopCalibration()
{
  pointingEngineMutex.lock();
  pointingEngine.stopCalibration();
  pointingEngineMutex.unlock();
}

void SkeletonPointing::checkCalibration()
{
  static const double minConditionNumber = 0.2;
  static const bool debug = false;
  if(display==NULL)
    return;
  vector<vector3d> dataPoints, dataVectors;

  pointingEngine.getCalibration(calibrationPoints, conditionNumbers, dataPoints, dataVectors);
  display->calibrationStatusUpdate(calibrationPoints, dataPoints, dataVectors);

  if(!display->isCalibrating())
    return;
  for(int i=0; i<3; i++){
    if(debug) printf("Calibration %d: %.3f, %.3f, %.3f conditionNumber:%.3f ready:%d\n",
      i,V3COMP(calibrationPoints[i]), conditionNumbers[i], (calibrationComplete[i])?1:0);
    if(conditionNumbers[i]>minConditionNumber){
      calibrationComplete[i] = true;
    }
  }
  if(debug) printf("\n");
}

void labeledDepthImageCallback(BYTE* img)
{
}

void rgbImageCallback(BYTE* img)
{
}

void skeletonCallback(NUI_SKELETON_FRAME* data)
{
  pointingThread.skeletonUpdate(data);
}

void startCalibrationCallback(int point)
{
  pointingThread.startCalibration(point);
}

void stopCalibrationCallback()
{
  pointingThread.stopCalibration();
}

void calibrationLoadSaveCallback(bool load)
{
  pointingThread.calibrationLoadSave(load);
}

void SkeletonPointing::calibrationLoadSave(bool load)
{
  if(load)
    pointingEngine.loadCalibration();
  else
    pointingEngine.saveCalibration();
}


void SkeletonPointing::publishData()
{
  static const bool debug = false;
  double hoverDuration=0.0;
  pointingEngine.getPointingTarget(target, target3d, hoverDuration);
  QByteArray msg("SkeletonPointing:");
  msg += QString::number(target.x,'f',3) +",";
  msg += QString::number(target.y,'f',3) +",";
  msg += QString::number(hoverDuration,'f',3) +";";
  net->writeDatagram(msg.data(), msg.size(), QHostAddress::Broadcast, 45000);
  display->updateTarget(target3d);
  if(debug) printf("transmitted: %s\n",msg.data());
}

void SkeletonPointing::run()
{
  static const bool debug = false;

  if(testMode){
    testDisplay();
    if(debug) printf("Terminating testMode thread. runApp:%d\n",runApp?1:0);
  }else{
    //Set up network interface
    net = new QUdpSocket(this);
    //Start up Kinect polling thread
    printf("Connecting to Kinect...\n");
    int retval = kinect.connect(NULL,&labeledDepthImageCallback,&skeletonCallback,&rgbImageCallback);
    printf(" returned value=%d\n",retval);
    display->setCallbacks(&startCalibrationCallback, &stopCalibrationCallback, &calibrationLoadSaveCallback);
    while(runApp){
      //FunctionTimer ft("App Main Loop");
      //Get Data from Kinect
      kinect.poll();
      //Run PointingEngine calibration
      checkCalibration();
      //Broadcast pointing data
      publishData();
      //Set GUI mode and send updated data
      Sleep(0.016);
    }
    if(debug) printf("Terminating thread. runApp:%d\n",runApp?1:0);
    kinect.close();
  }
  app->quit();
}

int main(int argc, char *argv[])
{
  static const bool debug = false;
  printf("\nSkeleton GUI\n\n");

  if(debug) printf("argc:%d\n", argc);
  for(int i=0; i<argc; i++){
    if(debug) printf("Arg %d: %s\n",i, argv[i]);
    if(strcmp(argv[i],"-t")==0)
      testMode = true;
  }

  printf("Starting up...\n");
  app = new QApplication(argc, argv);
  display = new SkeletonPointingGUI();
  //Set callback of gui calibrate event to pointingThread
  display->show();

  pointingThread.start();

  int retVal = app->exec();
  runApp = false;
  if(debug) printf("Waiting for thread termination... ");
  pointingThread.wait();
  if(debug) printf("Done. Bye Bye!\n");
  return retVal;
}
